//霍尔编码器，每个轮对应一个编码器，每个编码器对应两个引脚，一个是A相，一个是B相，A相和B相的信号是正交的，可以通过检测A相和B相的信号来判断转动方向和转动速度。
// TIM2 左
// TIM4 右


#include "encoder.h"
encoder_struct encoder = {0};

void Speed_PID_Init()
{
	Speed_PID.Kp = 0.01f;
	Speed_PID.Ki = 0.00f;
	Speed_PID.Kd = 0.00f;
	Speed_PID.integral_max = 500.0f;
}
void encoder_init(encoder_struct * encoder_struct)
{
	 HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
	 HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
}

void encoder_proc(encoder_struct * encoder_struct , uint16_t time)
{
	 static uint16_t last_time = 0;
	 if (HAL_GetTick() - last_time > time)
	 {
		 last_time = HAL_GetTick();
		 
		 encoder_struct -> encoder_l = -(int16_t)__HAL_TIM_GET_COUNTER(&htim2);
		 encoder_struct -> encoder_r = (int16_t)__HAL_TIM_GET_COUNTER(&htim4);


		 encoder_struct -> speed_l = (encoder_struct->encoder_l * 1000.0f) / time;
		 encoder_struct -> speed_r = (encoder_struct->encoder_r * 1000.0f) / time;
		 
		 __HAL_TIM_SET_COUNTER(&htim2, 0);
		 __HAL_TIM_SET_COUNTER(&htim4, 0);
		 
		 printf ("%d,%d\n",encoder_struct -> encoder_l,encoder_struct -> encoder_r);
//		 printf("%d\n", encoder_struct -> encoder_l);
	 }
}

void encoder_task()
{
	float taget_speed =10000.0f;
	float L_Out = PID_Calc(&Speed_PID,taget_speed,encoder.encoder_l,0);
	float R_Out = PID_Calc(&Speed_PID,taget_speed,encoder.encoder_r,0);
	
	motor_speed_set(L_Out,R_Out);
//	motor_speed_set(1,999);
	

//	 printf ("%.2f,%.2f\n",L_Out,R_Out);
}